///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_CQUAT_H_
#define _BUOLA_MAT_CQUAT_H_

#include <buola/buola.h>

#include <buola/mat/c3drotation.h>
#include <buola/mat.h>

namespace buola { namespace mat {

//!\addtogroup geometry
///@{

///! \brief name used to create a quaternion from a vector representing a
///! rotation in compressed form
DECLARE_NAME(CompressedRotation);

/////////////////////////////////////////////////////////////////////////////

/// rotation quaternion

/// Represents a quaternion which can be used for rotations. Operations only
/// work correctly for normalized (unit) quaternions, which are the only ones
/// which represent a rotation
///
/////////////////////////////////////////////////////////////////////////////

template<typename tScalar>
class CQuat
{
public:
//! \name constructors and destructor
//@{
    //! from the four component
    CQuat(tScalar pw=1.0,tScalar px=0.0,tScalar py=0.0,tScalar pz=0.0)
        :   w(pw)
        ,   x(px)
        ,   y(py)
        ,   z(pz)
    {}

    CQuat(tScalar pw,tScalar px,tScalar py,tScalar pz,bool pNormalize)
        :   w(pw)
        ,   x(px)
        ,   y(py)
        ,   z(pz)
    {
        if(pNormalize)
        {
            tScalar lNorm=sqrt(w*w+x*x+y*y+z*z);
            w/=lNorm;
            x/=lNorm;
            y/=lNorm;
            z/=lNorm;
        }
    }

    //! from a C3DRotation object
    explicit CQuat(const C3DRotation &pR)
    {
        tScalar lCosA=cos(pR.a/2),lSinA=sin(pR.a/2);
        tScalar lCosB=cos(pR.b/2),lSinB=sin(pR.b/2);
        tScalar lCosC=cos(pR.c/2),lSinC=sin(pR.c/2);

        w=lCosA*lCosB*lCosC+lSinA*lSinB*lSinC;
        x=lCosA*lCosB*lSinC-lSinA*lSinB*lCosC;
        y=lCosA*lSinB*lCosC+lSinA*lCosB*lSinC;
        z=lSinA*lCosB*lCosC-lCosA*lSinB*lSinC;
    }

    //! from a rotation matrix
    explicit CQuat(const tScalar pM[9])
    {
        tScalar lFourW2m1=pM[0]+pM[4]+pM[8];
        tScalar lFourX2m1=pM[0]-pM[4]-pM[8];
        tScalar lFourY2m1=-pM[0]+pM[4]-pM[8];
        tScalar lFourZ2m1=-pM[0]-pM[4]+pM[8];

        int lBigI=0;
        tScalar lBigFourm1=lFourW2m1;
        if(lFourX2m1>lBigFourm1)
        {
            lBigFourm1=lFourX2m1;
            lBigI=1;
        }
        if(lFourY2m1>lBigFourm1)
        {
            lBigFourm1=lFourY2m1;
            lBigI=2;
        }
        if(lFourZ2m1>lBigFourm1)
        {
            lBigFourm1=lFourZ2m1;
            lBigI=3;
        }

        tScalar lBigVal=sqrt(lBigFourm1+1.0)*0.5;
        tScalar lMult=0.25/lBigVal;

        switch(lBigI)
        {
        case 0:
            w=lBigVal;
            x=(pM[7]-pM[5])*lMult;
            y=(pM[2]-pM[6])*lMult;
            z=(pM[3]-pM[1])*lMult;
            break;
        case 1:
            x=lBigVal;
            w=(pM[7]-pM[5])*lMult;
            y=(pM[3]+pM[1])*lMult;
            z=(pM[2]+pM[6])*lMult;
            break;
        case 2:
            y=lBigVal;
            w=(pM[2]-pM[6])*lMult;
            x=(pM[3]+pM[1])*lMult;
            z=(pM[7]+pM[5])*lMult;
            break;
        case 3:
            z=lBigVal;
            w=(pM[3]-pM[1])*lMult;
            x=(pM[2]+pM[6])*lMult;
            y=(pM[7]+pM[5])*lMult;
            break;
        }
    }

    //! from a rotation matrix
    explicit CQuat(const CMat34d &pM)
    {
        double lFourW2m1=pM(0,0)+pM(1,1)+pM(2,2);
        double lFourX2m1=pM(0,0)-pM(1,1)-pM(2,2);
        double lFourY2m1=-pM(0,0)+pM(1,1)-pM(2,2);
        double lFourZ2m1=-pM(0,0)-pM(1,1)+pM(2,2);

        int lBigI=0;
        double lBigFourm1=lFourW2m1;
        if(lFourX2m1>lBigFourm1)
        {
            lBigFourm1=lFourX2m1;
            lBigI=1;
        }
        if(lFourY2m1>lBigFourm1)
        {
            lBigFourm1=lFourY2m1;
            lBigI=2;
        }
        if(lFourZ2m1>lBigFourm1)
        {
            lBigFourm1=lFourZ2m1;
            lBigI=3;
        }

        double lBigVal=sqrt(lBigFourm1+1.0)*0.5;
        double lMult=0.25/lBigVal;

        switch(lBigI)
        {
        case 0:
            w=lBigVal;
            x=(pM(2,1)-pM(1,2))*lMult;
            y=(pM(0,2)-pM(2,0))*lMult;
            z=(pM(1,0)-pM(0,1))*lMult;
            break;
        case 1:
            x=lBigVal;
            w=(pM(2,1)-pM(1,2))*lMult;
            y=(pM(1,0)+pM(0,1))*lMult;
            z=(pM(0,2)+pM(2,0))*lMult;
            break;
        case 2:
            y=lBigVal;
            w=(pM(0,2)-pM(2,0))*lMult;
            x=(pM(1,0)+pM(0,1))*lMult;
            z=(pM(2,1)+pM(1,2))*lMult;
            break;
        case 3:
            z=lBigVal;
            w=(pM(1,0)-pM(0,1))*lMult;
            x=(pM(0,2)+pM(2,0))*lMult;
            y=(pM(2,1)+pM(1,2))*lMult;
            break;
        }
    }

    //! \brief from a vector representing a rotation in compressed form
    //!
    //! The direction of the vector is the axis the rotation is done arount,
    //! while the modulus is the rotation angle, in radians
    CQuat(NCompressedRotation,const CVec<tScalar,3> &pV)
    {
        tScalar lTheta=norm(pV);
        if(lTheta==0)
        {
            w=1;
            x=y=z=0;
            return;
        }

        CVec<tScalar,3> lV=pV/lTheta;

        w=cos(lTheta/2);

        tScalar lSin=sin(lTheta/2);

        x=lSin*lV.x();
        y=lSin*lV.y();
        z=lSin*lV.z();
    }

    //! from a rotation angle and axis
    CQuat(tScalar pTheta,const CVec<tScalar,3> &pV,bool pNormalize=true)
    {
        w=cos(pTheta/2);

        tScalar lSin=sin(pTheta/2);

        if(pNormalize)
        {
            tScalar lInvNorm=1.0/norm(pV);
            x=lSin*pV.x()*lInvNorm;
            y=lSin*pV.y()*lInvNorm;
            z=lSin*pV.z()*lInvNorm;
        }
        else
        {
            x=lSin*pV.x();
            y=lSin*pV.y();
            z=lSin*pV.z();
        }
    }

    //! rotation that transforms vector \p p1 into \p p2
    CQuat(const CVec<tScalar,3> &p1,const CVec<tScalar,3> &p2)
    {
        CVec<tScalar,3> lV{p1.y()*p2.z()-p1.z()*p2.y(),
                           p1.z()*p2.x()-p1.x()*p2.z(),
                           p1.x()*p2.y()-p1.y()*p2.x()};

        tScalar lMod=norm(lV);

        if(lMod==tScalar(0.0))
        {
            if(dot(p1,p2)>=0)
            {
                w=1.0;
                x=0.0;
                y=0.0;
                z=0.0;
            }
            else
            {
                mat::CVec<tScalar,3> lOther;
                if(p1.x()<p1.y())
                    lOther=cross(p1,mat::CVec<tScalar,3>(1,0,0));
                else
                    lOther=cross(p1,mat::CVec<tScalar,3>(0,1,0));
                lOther/=norm(lOther);
                w=0;
                x=lOther.x();
                y=lOther.y();
                z=lOther.z();
            }
        }
        else
        {
            tScalar lTheta=atan2(lMod,dot(p1,p2));

            w=cos(lTheta/2);

            tScalar lSin=sin(lTheta/2);

            x=lSin*lV.x()/lMod;
            y=lSin*lV.y()/lMod;
            z=lSin*lV.z()/lMod;
        }
    }
//@}

    //! assignment operator
    CQuat &operator=(const CQuat &pRH)=default;

    //! \brief adds two quaternions (in-place version)
    //!
    //! \warning The result will not be a unit quaternion, so it should only be
    //! used as a intermediate result for some operation. Most of the functions
    //! of the CQuat class will not produce the correct results with
    //! this kind of quaternion
    CQuat &operator+=(const CQuat &pRH)
    {
        w+=pRH.w;
        x+=pRH.x;
        y+=pRH.y;
        z+=pRH.z;

        return *this;
    }

    //! \brief subtracts two quaternions (in-place version)
    //!
    //! \warning The result will not be a unit quaternion, so it should only be
    //! used as a intermediate result for some operation. Most of the functions
    //! of the CQuat class will not produce the correct results with
    //! this kind of quaternion
    CQuat &operator-=(const CQuat &pRH)
    {
        w-=pRH.w;
        x-=pRH.x;
        y-=pRH.y;
        z-=pRH.z;

        return *this;
    }

    //! \brief gets the rotation as a compressed vector
    //!
    //! \sa CQuat(TCompressedRotation,const CVec&) for information on the format
    CVec<tScalar,3> GetCompressedVector() const
    {
        if(w>1.0)
            return CQuat(1.0,x,y,z).GetCompressedVector();
        else if(w<-1.0)
            return CQuat(-1.0,x,y,z).GetCompressedVector();
        tScalar lAngle=acos(w);
        tScalar lTheta=lAngle*2;
        CVec<tScalar,3> lRet{x/sin(lAngle),y/sin(lAngle),z/sin(lAngle)};

        if(lTheta>M_PI)
        {
//            lRet=-lRet;
            lTheta=lTheta-2*M_PI;
        }

        return lRet*lTheta;
    }

    //! \brief gets the rotation axis
    CVec<tScalar,3> GetAxis() const
    {
        if(w>1.0)
            return CQuat(1.0,x,y,z).GetCompressedVector();
        else if(w<-1.0)
            return CQuat(-1.0,x,y,z).GetCompressedVector();
        tScalar lAngle=acos(w);
        return {x/sin(lAngle),y/sin(lAngle),z/sin(lAngle)};
    }

    explicit operator C3DRotation() const
    {
        return C3DRotation(atan2(2*(x*y+w*z),w*w+x*x-y*y-z*z),
                           asin(-2*(x*z-w*y)),
                           atan2(2*(w*x+y*z),w*w-x*x-y*y+z*z));
    }

    
    //! \name quaternion components
    //@{
    tScalar w,x,y,z;
    //@}
};

//! \relates CQuat
//! \brief negates the quaternion
//!
//! This only affects the sign of the components. The quaternion will still
//! represent the same rotation
template<typename tScalar>
inline CQuat<tScalar> operator-(const CQuat<tScalar> &pQ)
{
    return CQuat<tScalar>(-pQ.w,-pQ.x,-pQ.y,-pQ.z);
}

//! \relates CQuat
//! \brief returns the conjugate of the quaternion
//!
//! The conjugate quaternion represents a rotation in the opposite direction
template<typename tScalar>
inline CQuat<tScalar> conj(const CQuat<tScalar> &pQ)
{
    return CQuat<tScalar>(pQ.w,-pQ.x,-pQ.y,-pQ.z);
}

//! \relates CQuat
//! \brief returns the absolute value of the quaternion
//!
//! Should be 1 for quaternions that represent a rotation
template<typename tScalar>
inline tScalar abs(const CQuat<tScalar> &pQ)
{
    return sqrt(pQ.w*pQ.w+pQ.x*pQ.x+pQ.y*pQ.y+pQ.z*pQ.z);
}

//! \relates CQuat
//! \brief returns the exponential of a quaternion
template<typename tScalar>
inline CQuat<tScalar> pow(const CQuat<tScalar> &pQ,tScalar pE)
{
    CQuat<tScalar> lQ=pQ;
    if(lQ.w<0) lQ=-lQ;
    if(lQ.w>1.0)
        lQ.w=1.0;

    tScalar lAlpha=acos(lQ.w);
    if(lAlpha==0.0) return lQ;
    tScalar lNewAlpha=lAlpha*pE;
    tScalar lSinAlpha=sin(lAlpha);
    tScalar lMult=(lSinAlpha==0.0)?(lNewAlpha/lAlpha):(sin(lNewAlpha)/lSinAlpha);
    return {cos(lNewAlpha),lQ.x*lMult,lQ.y*lMult,lQ.z*lMult};
}

//! \relates CQuat
//! \brief returns the angle of the rotation represented by the quaternion
template<typename tScalar>
inline tScalar arg(const CQuat<tScalar> &pQ)
{
    if(pQ.w>1.0)
        return arg(CQuat<tScalar>(1.0,pQ.x,pQ.y,pQ.z));
    else if(pQ.w<-1.0)
        return arg(CQuat<tScalar>(-1.0,pQ.x,pQ.y,pQ.z));
    tScalar lRet=acos(pQ.w)*2;
    if(lRet>M_PI)
        return lRet-2*M_PI;
    return lRet;
}

//! \relates CQuat
//! \brief multiplies two quaternion
//!
//! The rotation represented by the resulting quaternion is equivalent to the
//! concatenation of both rotations
template<typename tScalar>
inline CQuat<tScalar> operator*(const CQuat<tScalar> &pLH,const CQuat<tScalar> &pRH)
{
    return {pLH.w*pRH.w-pLH.x*pRH.x-pLH.y*pRH.y-pLH.z*pRH.z,
            pLH.w*pRH.x+pLH.x*pRH.w-pLH.z*pRH.y+pLH.y*pRH.z,
            pLH.w*pRH.y+pLH.y*pRH.w-pLH.x*pRH.z+pLH.z*pRH.x,
            pLH.w*pRH.z+pLH.z*pRH.w-pLH.y*pRH.x+pLH.x*pRH.y};
}

//! \relates CQuat
//! \relates CVec
//! \brief rotates a vector
template<typename tScalar>
inline CVec<tScalar,3> operator*(const CQuat<tScalar> &pQ,const CVec<tScalar,3> &pV)
{
    return {pV.x()*(1-2*pQ.y*pQ.y-2*pQ.z*pQ.z)+pV.y()*(2*pQ.x*pQ.y-2*pQ.w*pQ.z)  +pV.z()*(2*pQ.x*pQ.z+2*pQ.w*pQ.y),
            pV.x()*(2*pQ.x*pQ.y+2*pQ.w*pQ.z)  +pV.y()*(1-2*pQ.x*pQ.x-2*pQ.z*pQ.z)+pV.z()*(2*pQ.y*pQ.z-2*pQ.w*pQ.x),
            pV.x()*(2*pQ.x*pQ.z-2*pQ.w*pQ.y)  +pV.y()*(2*pQ.y*pQ.z+2*pQ.w*pQ.x)  +pV.z()*(1-2*pQ.x*pQ.x-2*pQ.y*pQ.y)};
}

template<typename tScalar>
inline io::CTextWriter &operator<<(io::CTextWriter &pWriter,const CQuat<tScalar> &pQ)
{
    pWriter << '(' << pQ.w << ' ' << pQ.x << ' ' << pQ.y << ' ' << pQ.z << ')';
    return pWriter;
}

template<typename tScalar>
inline io::CTextReader &operator>>(io::CTextReader &pReader,CQuat<tScalar> &pQ)
{
    char lDummy;
    pReader >> lDummy >> pQ.w >> pQ.x >> pQ.y >> pQ.z >> lDummy;
    return pReader;
}

///@}

typedef CQuat<double> CQuat_d;
typedef CQuat<float> CQuat_f;

/*namespace mat*/ } /*namespace buola*/ }

#endif
